/** Display organization
 *
 * This file contains display organization, i.e. the function to display
 * only the required texts.
*/
//##############################################################################




/** For sprintf */
#include <stdio.h>
//------------------------------------------------------------------------------




/** For display content manipulation */
#include "LR_display.h"
//------------------------------------------------------------------------------




/** Control the associated timer */
#include "LR_timer.h"
//------------------------------------------------------------------------------




/** Globals to display */
#include "globals.h"
//##############################################################################




/** Timer slot (numer) which is used by the display handler */
volatile unsigned char slot;
//------------------------------------------------------------------------------




/** Current page displayed */
volatile unsigned char page = 6;
//------------------------------------------------------------------------------




/** Current position (row) of the cursor */
volatile unsigned char cursor = 0;
//------------------------------------------------------------------------------




/** Page description strings displayed at the bottom of each page */
char page_desc [] [ 13 ] = {
  "position PID",
  "yaw      PID",
  "attitude PID",
  "quaternion  ",
  "1D rotation ",
  "raw sensors ",
  "control 1   ",
  "control 2   "
};
//##############################################################################




/** Actual display handler.
 *
 * This function gets called by a timer interrupt. It then displays the actual
 * page on the screen.
*/
//__attribute__((__interrupt__))
static void display_handler( void )
{
  char string[4][25];
  if (page == 0)
  {
    // Position pid parameters
    sprintf( string[0], " P: ...TODO" );
    sprintf( string[1], " I: ..." );
    sprintf( string[2], " D: ..." );
  }
  else if (page == 1)
  {
    // yaw pid parameters
    sprintf( string[0], " P: %10.10g", pid_yaw[0] );
    sprintf( string[1], " I: %10.10g", pid_yaw[1] );
    sprintf( string[2], " D: %10.10g", pid_yaw[2] );
  }
  else if (page == 2)
  {
    // attitude pid parameters
    sprintf( string[0], " P: %10.10g", pid_attitude[0] );
    sprintf( string[1], " I: %10.10g", pid_attitude[1] );
    sprintf( string[2], " D: %10.10g", pid_attitude[2] );
  }
  else if (page == 3)
  {
    // quaternion display
    sprintf( string[0], "q1: %10.10g %5.5g", quaternion[1], quaternion[0] );
    sprintf( string[1], "q2: %10.10g", quaternion[2] );
    sprintf( string[2], "q3: %10.10g", quaternion[3] );
  }
  else if (page == 4)
  {
    // print 1D rotation angles
    sprintf( string[0], "X: %4.4g %4.4g", rot1D[0], position[0] );
    sprintf( string[1], "Y: %4.4g %4.4g", rot1D[1], position[1] );
    sprintf( string[2], "Z: %4.4g %4.4g", rot1D[2], position[2] );
  }
  else if (page == 5)
  {
    // raw sensor values
    sprintf( string[0], "x: %4.4g %4.4g", itg[0], adxl[0] );
    sprintf( string[1], "y: %4.4g %4.4g", itg[1], adxl[1] );
    sprintf( string[2], "z: %4.4g %4.4g", itg[2], adxl[2] );
  }
  else if (page == 6)
  {
    // controll terminal
    sprintf( string[0], " engines: %s      ", (ctl_engine) ? ("on ") : ("off") );
    sprintf( string[1], " calibrate: %s   ",
        (ctl_calibration) ? ("... ") : ("done") );
    sprintf( string[2], " quaternion: %s",
        (ctl_quaternion) ? ("sending") : ("stopped") );
  }
  else if (page == 7)
  {
    sprintf( string[0], " attitude:%s", (ctl_attitude) ? ("on ") : ("off") );
    sprintf( string[1], " yaw:     %s", (ctl_yaw) ? ("on ") : ("off") );
    sprintf( string[2], " pos:     %s", (ctl_position) ? ("on ") : ("off") );
  }
  // Print foot line
  sprintf( string[3], "%d/%d %s", page + 1, 8,
      page_desc[page] );
  // Update the display
  LR_display_write(0, 0, string[0] );
  LR_display_write(0, 1, string[1] );
  LR_display_write(0, 2, string[2] );
  LR_display_write(0, 3, string[3] );
  if (page < 3 || page > 5)
  { 
    LR_display_write(0, cursor, "+" );
  }
  //LR_timer_sti( slot );
}
//------------------------------------------------------------------------------





/** Helper function to start the display handler.
 *
 * This function is inteded to use when starting the display handler.
 *
 * \parm s Timer slot number
 * \parm period Timer period in milliseconds
*/
void display_handler_start( unsigned char s, unsigned int period )
{
  slot = s;
  page = 6;
  cursor = 0;
  LR_timer_assign( slot, display_handler, period );
  LR_timer_start( slot );
}
//------------------------------------------------------------------------------




/** Move the cursor up one row, if possible */
void display_handler_cup( void )
{
  if (cursor > 0)
  {
    cursor--;
  }
}
//------------------------------------------------------------------------------




/** Move the cursor down one row, if possible */
void display_handler_cdown( void )
{
  if (cursor < 2)
  {
    cursor++;
  }
}
//------------------------------------------------------------------------------




/** Get the current position (row) of the cursor.
 *
 * \return Current cursor position.
*/
unsigned int display_handler_cget( void )
{
  return cursor;
}
//------------------------------------------------------------------------------




/** Show previous page, if possible */
void display_handler_pleft( void )
{
  if (page > 0)
  {
    page--;
  }
}
//------------------------------------------------------------------------------




/** Show next page, if possible */
void display_handler_pright( void )
{
  if (page < 7)
  {
    page++;
  }
}
//------------------------------------------------------------------------------




/** Get current page number.
 *
 * \return Current page number.
*/
unsigned int display_handler_pget( void )
{
  return page;
}
//##############################################################################


void display_handler_run( void )
{
  display_handler();
}

